/**
 * @file magnetometer_adaption.h
 * @author your name (you@domain.com)
 * @brief 
 * @version 0.1
 * @date 2022-12-21
 * 
 * @copyright Copyright (c) 2022
 * 
 */

#ifndef _MAGNETOMETER_ADAPTION_H
#define _MAGNETOMETER_ADAPTION_H

#include "system_queue_message.h"
#include "stdint.h"

/**
 * @brief 三维点结构体
 * 
 */
struct Point3D
{
    double x;
    double y;
    double z;
};

/**
 * @brief 球面拟合结构体
 * 
 */
typedef struct __SphereLeastFitStruct
{
	enum {
		NotInit,
		NotComplete,
		Completed
	}calibration_status;
    uint32_t point_num;             // 输入点总数
    // 拟合结果
    struct
    {
        struct Point3D center_point;    // 中心点坐标
        double radius;                  // 球面半径
    }fitting_result;

    // 过程变量
    struct
    {
        double sum_x;   // sum(x)
        double sum_y;
        double sum_z;

        double sum_xx;  // sum(x^2)
        double sum_yy;
        double sum_zz;

        double sum_xy;  // sum(x*y)
        double sum_xz;
        double sum_yz;

        double sum_xxx; // sum(x^3)
        double sum_yyy;
        double sum_zzz;

        double sum_xyy; // sum(x*y*y)
        double sum_xxy;
        double sum_xzz;
        double sum_xxz;
        double sum_yyz;
        double sum_yzz;
    }process_variables;

}SphereLeastFitStruct;

typedef struct __MagnetometerTypedef
{
    /* 原始数据 */
    int16_t     mag_raw[3];

	/* 经方向转换和但单位转换后的数据 */
	float 		mag_convert[3];

	float 		mag_rel[3];
    /* 用于消息队列的mag数据结构 */
    sensor_mag_msg mag_msg;
    float test_yaw;

	float 		mag_offset[3];

	int8_t		direction[6];
    
    /* 数据采集的时间和频率 */
	uint64_t 	start_record_time_us;
	uint64_t 	record_time_us;
	uint64_t 	last_record_time_us;
	uint32_t	derta_record_time_us;
	uint16_t	update_freq_Hz;

	SphereLeastFitStruct sphere_fit_handle;
}MagnetometerTypedef;



int8_t Magnetometer_Init(MagnetometerTypedef *_magnetometer);

void Magnetometer_GetMagRaw(MagnetometerTypedef *_magnetometer, int16_t *mag_raw);

void Magnetometer_GetMagRel(MagnetometerTypedef *_magnetometer, float *mag_rel);

int8_t Magnetometer_UpdateData(MagnetometerTypedef *_magnetometer);

int8_t Magnetometer_CheckConnect(MagnetometerTypedef *_magnetometer);

int8_t Magnetometer_Restart(MagnetometerTypedef *_magnetometer);


#endif /* _MAGNETOMETER_ADAPTION_H */

